#include <vector>
#include <algorithm>

// 使用float类型表示实数
using Real = float;

// 节点通信模块中的常量
const int NRBO = /* 定义NRBO的值 */;
const int NRBO_DISTRIB = /* 定义NRBO_DISTRIB的值 */;
const int NRBO_TYPE4 = /* 定义NRBO_TYPE4的值 */;

// 阻尼通信模块中的常量
const int IDMP = /* 定义IDMP的值 */;
const int NSND_RB = /* 定义NSND_RB的值 */;
const int NRCV_RB = /* 定义NRCV_RB的值 */;

// 常量定义
const Real ZERO = 0.0f;
const Real ONE = 1.0f;

/**
 * @brief 计算加速度的子程序，用于处理刚体运动
 * @param velocity 速度数组
 * @param acceleration 加速度数组
 * @param blambda 旋转矩阵数组
 * @param rigidBodyNodes 刚体节点索引数组
 * @param rigidBodyNodePointers 刚体节点指针数组
 * @param rigidBodyType 刚体类型数组
 * @param failureParams 失效参数数组
 * @param dkg 位移梯度数组
 * @param dt2 时间步长平方
 * @param dtx 时间步长
 * @param lcg 刚体中心索引数组
 * @param dxrb 刚体节点位移数组
 * @param rcgd 刚体中心数据数组
 * @param nodeRigidBody 节点所属刚体数组
 * @param sendBufferRB 发送缓冲区
 * @param receiveBufferRB 接收缓冲区
 */
void ACCEL4(std::vector<Real>& velocity,
            std::vector<Real>& acceleration,
            std::vector<Real>& blambda,
            std::vector<int>& rigidBodyNodes,
            std::vector<int>& rigidBodyNodePointers,
            std::vector<int>& rigidBodyType,
            std::vector<std::vector<Real>>& failureParams,
            std::vector<std::vector<Real>>& dkg,
            Real dt2,
            Real dtx,
            std::vector<int>& lcg,
            std::vector<std::vector<Real>>& dxrb,
            std::vector<std::vector<Real>>& rcgd,
            std::vector<int>& nodeRigidBody,
            std::vector<Real>& sendBufferRB,
            std::vector<Real>& receiveBufferRB) {

    // 局部变量
    std::vector<Real> W(9);           // 工作数组
    std::vector<Real> VC(6);          // 速度数组
    std::vector<Real> A1(2);          // 失效参数1
    std::vector<Real> A2(2);          // 失效参数2
    std::vector<Real> ALP1(3);        // 插值系数1
    std::vector<Real> ALP2(3);        // 插值系数2

    // 1. 将加速度值存入刚体中心数据数组，并进行广播（因为PBMOTION可能改变了中心值）
    for (int je = 0; je < NRBO; ++je) {
        if (rigidBodyType[2 * NRBO + je] == 0) continue;
        int cogIndex = lcg[je];
        if (cogIndex != 0) {
            int offset = 6 * (cogIndex - 1);
            for (int i = 0; i < 6; ++i) {
                rcgd[i][je] = acceleration[offset + i];
            }
        }
    }

    // 如果所有刚体都是TYPE4类型，则直接返回
    if (NRBO == NRBO_TYPE4) return;

    // 仅对分布式刚体进行通信
    if (IDMP == 1 && NRBO_DISTRIB != 0) {
        // 调用MPP通信函数（此处为示意，实际需要实现通信逻辑）
        MPP_RBCOMMR(rcgd, sendBufferRB, NSND_RB, receiveBufferRB, NRCV_RB, 36, 1, 6, 0, 0, 0, 1, 3639, 3639);
    }

    Real dtxInverse = 1.0f / dtx;

    // 遍历所有刚体
    for (int je = 0; je < NRBO; ++je) {
        if (nodeRigidBody[je] == 0) continue;
        if (rigidBodyType[je] == 4) continue;

        int startIndex = rigidBodyNodes[je];
        int endIndex = rigidBodyNodes[je + 1] - 1;
        int numNodes = endIndex - startIndex + 1;

        // 检查是否为完全失效的焊点或铆钉
        int triggerType = rigidBodyType[2 * NRBO + je];
        if (triggerType == 0 || rigidBodyType[je] < 0) continue;

        int i1 = 0;
        int i2 = 0;

        // 检查是否为类型1或2的焊点/铆钉
        bool isRivetOrSpotweld = ((rigidBodyType[je] == 1 && failureParams[1][je] >= 0.0f) ||
                                 rigidBodyType[je] == 2);

        if (isRivetOrSpotweld) {
            if (failureParams[6][je] >= 1.0f && failureParams[7][je] >= 1.0f) continue;
        } else if (rigidBodyType[je] == 1 && failureParams[1][je] < 0.0f) {
            // 能量吸收焊点
            if (failureParams[6][je] > 0.0f && failureParams[7][je] > 0.0f) {
                continue;
            } else if (failureParams[6][je] > 0.0f) {
                i1 = 1;
            } else if (failureParams[7][je] > 0.0f && numNodes == 3) {
                i2 = 1;
            }
        }

        // 计算节点位移
        int nc = 0;
        int nh = 0;
        RBDISP(startIndex, numNodes, nc, nh, rcgd[24][je], dkg, rcgd[6][je], rcgd[0][je], dt2, dtx, dxrb, W, VC);

        // 计算刚体中心的旋转加速度（全局坐标系）
        Real acgX = rcgd[24][je] * rcgd[3][je] + rcgd[27][je] * rcgd[4][je] + rcgd[30][je] * rcgd[5][je];
        Real acgY = rcgd[25][je] * rcgd[3][je] + rcgd[28][je] * rcgd[4][je] + rcgd[31][je] * rcgd[5][je];
        Real acgZ = rcgd[26][je] * rcgd[3][je] + rcgd[29][je] * rcgd[4][je] + rcgd[32][je] * rcgd[5][je];

        // 计算末端节点的新加速度
        if (isRivetOrSpotweld) {
            A1[0] = failureParams[6][je] * failureParams[6][je];
            A1[0] = std::min(ONE, A1[0]);
            A2[0] = 1.0f - A1[0];
            A2[0] = std::max(ZERO, A2[0]);

            A1[1] = failureParams[7][je] * failureParams[7][je];
            A1[1] = std::min(ONE, A1[1]);
            A2[1] = 1.0f - A1[1];
            A2[1] = std::max(ZERO, A2[1]);

            ALP1[0] = A1[0];
            ALP2[0] = A2[0];
            ALP1[1] = std::min(A1[0], A1[1]);
            ALP2[1] = std::max(A2[0], A2[1]);
            ALP1[2] = A1[1];
            ALP2[2] = A2[1];

            if (rigidBodyType[je] == 1) {
                for (int i = 0; i < numNodes; ++i) {
                    int nodeIndex = rigidBodyNodePointers[startIndex + i - 1];
                    int accelOffset = 6 * (nodeIndex - 1);
                    Real alpha1 = ALP1[i];
                    Real alpha2 = ALP2[i];

                    Real vp1 = velocity[accelOffset];
                    Real vp2 = velocity[accelOffset + 1];
                    Real vp3 = velocity[accelOffset + 2];
                    Real dkvX = dxrb[0][i];
                    Real dkvY = dxrb[1][i];
                    Real dkvZ = dxrb[2][i];
                    Real acspXT = (dkvX - vp1) * dtxInverse;
                    Real acspYT = (dkvY - vp2) * dtxInverse;
                    Real acspZT = (dkvZ - vp3) * dtxInverse;

                    acceleration[accelOffset] = alpha1 * acceleration[accelOffset] + alpha2 * acspXT;
                    acceleration[accelOffset + 1] = alpha1 * acceleration[accelOffset + 1] + alpha2 * acspYT;
                    acceleration[accelOffset + 2] = alpha1 * acceleration[accelOffset + 2] + alpha2 * acspZT;

                    int lambdaOffset = 9 * (nodeIndex - 1);
                    if (alpha1 != 0.0f) {
                        Real acjX = blambda[lambdaOffset] * acceleration[accelOffset + 3] +
                                   blambda[lambdaOffset + 3] * acceleration[accelOffset + 4] +
                                   blambda[lambdaOffset + 6] * acceleration[accelOffset + 5];
                        Real acjY = blambda[lambdaOffset + 1] * acceleration[accelOffset + 3] +
                                   blambda[lambdaOffset + 4] * acceleration[accelOffset + 4] +
                                   blambda[lambdaOffset + 7] * acceleration[accelOffset + 5];
                        Real acjZ = blambda[lambdaOffset + 2] * acceleration[accelOffset + 3] +
                                   blambda[lambdaOffset + 5] * acceleration[accelOffset + 4] +
                                   blambda[lambdaOffset + 8] * acceleration[accelOffset + 5];
                        acjX = alpha1 * acjX + alpha2 * acgX;
                        acjY = alpha1 * acjY + alpha2 * acgY;
                        acjZ = alpha1 * acjZ + alpha2 * acgZ;
                        acceleration[accelOffset + 3] = blambda[lambdaOffset] * acjX +
                                                      blambda[lambdaOffset + 1] * acjY +
                                                      blambda[lambdaOffset + 2] * acjZ;
                        acceleration[accelOffset + 4] = blambda[lambdaOffset + 3] * acjX +
                                                      blambda[lambdaOffset + 4] * acjY +
                                                      blambda[lambdaOffset + 5] * acjZ;
                        acceleration[accelOffset + 5] = blambda[lambdaOffset + 6] * acjX +
                                                      blambda[lambdaOffset + 7] * acjY +
                                                      blambda[lambdaOffset + 8] * acjZ;
                    } else {
                        acceleration[accelOffset + 3] = blambda[lambdaOffset] * acgX +
                                                      blambda[lambdaOffset + 1] * acgY +
                                                      blambda[lambdaOffset + 2] * acgZ;
                        acceleration[accelOffset + 4] = blambda[lambdaOffset + 3] * acgX +
                                                      blambda[lambdaOffset + 4] * acgY +
                                                      blambda[lambdaOffset + 5] * acgZ;
                        acceleration[accelOffset + 5] = blambda[lambdaOffset + 6] * acgX +
                                                      blambda[lambdaOffset + 7] * acgY +
                                                      blambda[lambdaOffset + 8] * acgZ;
                    }
                }
            } else {
                for (int i = 0; i < numNodes; ++i) {
                    int nodeIndex = rigidBodyNodePointers[startIndex + i - 1];
                    int accelOffset = 6 * (nodeIndex - 1);
                    Real alpha1 = ALP1[i];
                    Real alpha2 = ALP2[i];

                    Real vp1 = velocity[accelOffset];
                    Real vp2 = velocity[accelOffset + 1];
                    Real vp3 = velocity[accelOffset + 2];
                    Real dkvX = dxrb[0][i];
                    Real dkvY = dxrb[1][i];
                    Real dkvZ = dxrb[2][i];
                    Real acspXT = (dkvX - vp1) * dtxInverse;
                    Real acspYT = (dkvY - vp2) * dtxInverse;
                    Real acspZT = (dkvZ - vp3) * dtxInverse;

                    acceleration[accelOffset] = alpha1 * acceleration[accelOffset] + alpha2 * acspXT;
                    acceleration[accelOffset + 1] = alpha1 * acceleration[accelOffset + 1] + alpha2 * acspYT;
                    acceleration[accelOffset + 2] = alpha1 * acceleration[accelOffset + 2] + alpha2 * acspZT;
                }
            }
        } else {
            for (int i = i1; i < numNodes - i2; ++i) {
                int nodeIndex = rigidBodyNodePointers[startIndex + i - 1];
                int accelOffset = 6 * (nodeIndex - 1);
                int lambdaOffset = 9 * (nodeIndex - 1);

                Real vp1 = velocity[accelOffset];
                Real vp2 = velocity[accelOffset + 1];
                Real vp3 = velocity[accelOffset + 2];
                Real dkvX = dxrb[0][i];
                Real dkvY = dxrb[1][i];
                Real dkvZ = dxrb[2][i];

                acceleration[accelOffset] = (dkvX - vp1) * dtxInverse;
                acceleration[accelOffset + 1] = (dkvY - vp2) * dtxInverse;
                acceleration[accelOffset + 2] = (dkvZ - vp3) * dtxInverse;
                acceleration[accelOffset + 3] = blambda[lambdaOffset] * acgX +
                                              blambda[lambdaOffset + 1] * acgY +
                                              blambda[lambdaOffset + 2] * acgZ;
                acceleration[accelOffset + 4] = blambda[lambdaOffset + 3] * acgX +
                                              blambda[lambdaOffset + 4] * acgY +
                                              blambda[lambdaOffset + 5] * acgZ;
                acceleration[accelOffset + 5] = blambda[lambdaOffset + 6] * acgX +
                                              blambda[lambdaOffset + 7] * acgY +
                                              blambda[lambdaOffset + 8] * acgZ;
            }
        }
    }
}

// 辅助函数声明
void MPP_RBCOMMR(std::vector<std::vector<Real>>& rcgd,
                 std::vector<Real>& sendBuffer,
                 int sendCount,
                 std::vector<Real>& receiveBuffer,
                 int receiveCount,
                 int totalSize,
                 int startIndex,
                 int endIndex,
                 int option1,
                 int option2,
                 int option3,
                 int option4,
                 int tag1,
                 int tag2);

void RBDISP(int startIndex,
            int numNodes,
            int nc,
            int nh,
            std::vector<Real>& blambda,
            std::vector<std::vector<Real>>& dkg,
            std::vector<Real>& velocity,
            std::vector<Real>& acceleration,
            Real dt2,
            Real dtx,
            std::vector<std::vector<Real>>& dxrb,
            std::vector<Real>& W,
            std::vector<Real>& VC);